
#if 1
#include <Arduino.h>
#include "bldc_init.h"


// BLDC motor & driver instance
BLDCMotor motor0 = BLDCMotor(11);
BLDCMotor motor1 = BLDCMotor(11);

// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0

// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver0 = BLDCDriver3PWM(M0_PWMA_PIN, M0_PWMB_PIN, M0_PWMC_PIN, M0_PWM_EN);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWMA_PIN, M1_PWMB_PIN, M1_PWMC_PIN, M1_PWM_EN);

// instantiate the commander

float g_target_velocity = 0.0;


Commander command = Commander(Serial);
// void doMotor0(char* cmd) { command.motor(&motor0, cmd); }
// void doMotor1(char* cmd) { command.motor(&motor1, cmd); }
void doTarget(char* cmd) { command.scalar(&g_target_velocity, cmd); }

void config_motor0(void)
{  // if SimpleFOCMini is stacked in arduino headers
  // on pins 8,11,10,9,8 
  // pin 8 is used as ground
  pinMode(M0_PWM_EN,OUTPUT);
  pinMode(M0_PWM_EN,LOW);

  // driver config
  // power supply voltage [V]
  driver0.voltage_power_supply = 12;
  driver0.init();
  // link the motor and the driver
  motor0.linkDriver(&driver0);

  // aligning voltage [V]
  motor0.voltage_sensor_align = 3;

  // set motion control loop to be used
  motor0.controller = MotionControlType::velocity_openloop;

  // default voltage_power_supply
  motor0.voltage_limit = 6; // Volts

  // initialize motor
  motor0.init();
  // align encoder and start FOC
  motor0.initFOC();
}



void config_motor1(void)
{  // if SimpleFOCMini is stacked in arduino headers
  // on pins 8,11,10,9,8 
  // pin 8 is used as ground
  pinMode(M1_PWM_EN,OUTPUT);
  pinMode(M1_PWM_EN,LOW);

  // driver config
  // power supply voltage [V]
  driver1.voltage_power_supply = 12;
  driver1.init();
  // link the motor and the driver
  motor1.linkDriver(&driver1);

  // aligning voltage [V]
  motor1.voltage_sensor_align = 3;

  // set motion control loop to be used
  motor1.controller = MotionControlType::velocity_openloop;

  // default voltage_power_supply
  motor1.voltage_limit = 6; // Volts

  // initialize motor
  motor1.init();
  // align encoder and start FOC
  motor1.initFOC();
}
#endif 


void config_bldc_cmd(void)
{
    // use monitoring with serial 
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
  SimpleFOCDebug::enable(&Serial);

  // comment out if not needed
  //注意此处：可能只有一个串口是有效的
  motor0.useMonitoring(Serial);
  motor1.useMonitoring(Serial);

  // add target command M
  command.add('M', doTarget, "motor");
  command.add('M', doTarget, "motor");


  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity using serial terminal:"));
  
  //motor0.target = 1; //initial target velocity 1 rad/s
  //motor1.target = 1; //initial target velocity 1 rad/s

  Serial.println("Target velocity: 1 rad/s");
  Serial.println("Voltage limit 2V");
  _delay(1000);
}